/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install, copy or use the software.
//
// Copyright (C) 2009, Farhad Dadgostar
// Intel Corporation and third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

CvFuzzyPoint::CvFuzzyPoint(double _x, double _y) {
    x = _x;
    y = _y;
};

bool CvFuzzyCurve::between(double x, double x1, double x2) {
    if ((x >= x1) && (x <= x2)) {
        return true;
    } else if ((x >= x2) && (x <= x1)) {
        return true;
    }

    return false;
};

CvFuzzyCurve::CvFuzzyCurve() {
    value = 0;
};

CvFuzzyCurve::~CvFuzzyCurve() {
    // nothing to do
};

void CvFuzzyCurve::setCentre(double _centre) {
    centre = _centre;
};

double CvFuzzyCurve::getCentre() {
    return centre;
};

void CvFuzzyCurve::clear() {
    points.clear();
};

void CvFuzzyCurve::addPoint(double x, double y) {
    CvFuzzyPoint* point;
    point = new CvFuzzyPoint(x, y);
    points.push_back(*point);
};

double CvFuzzyCurve::calcValue(double param) {
    int size = (int)points.size();
    double x1, y1, x2, y2, m, y;
    for (int i = 1; i < size; i++) {
        x1 = points[i - 1].x;
        x2 = points[i].x;
        if (between(param, x1, x2)) {
            y1 = points[i - 1].y;
            y2 = points[i].y;
            if (x2 == x1) {
                return y2;
            }
            m = (y2 - y1) / (x2 - x1);
            y = m * (param - x1) + y1;
            return y;
        }
    }
    return 0;
};

double CvFuzzyCurve::getValue() {
    return value;
};

void CvFuzzyCurve::setValue(double _value) {
    value = _value;
};


CvFuzzyFunction::CvFuzzyFunction() {
    // nothing to do
};

CvFuzzyFunction::~CvFuzzyFunction() {
    curves.clear();
};

void CvFuzzyFunction::addCurve(CvFuzzyCurve* curve, double value) {
    curves.push_back(*curve);
    curve->setValue(value);
};

void CvFuzzyFunction::resetValues() {
    int numCurves = (int)curves.size();
    for (int i = 0; i < numCurves; i++) {
        curves[i].setValue(0);
    }
};

double CvFuzzyFunction::calcValue() {
    double s1 = 0, s2 = 0, v;
    int numCurves = (int)curves.size();
    for (int i = 0; i < numCurves; i++) {
        v = curves[i].getValue();
        s1 += curves[i].getCentre() * v;
        s2 += v;
    }

    if (s2 != 0) {
        return s1 / s2;
    } else {
        return 0;
    }
};

CvFuzzyCurve* CvFuzzyFunction::newCurve() {
    CvFuzzyCurve* c;
    c = new CvFuzzyCurve();
    addCurve(c);
    return c;
};

CvFuzzyRule::CvFuzzyRule() {
    fuzzyInput1 = NULL;
    fuzzyInput2 = NULL;
    fuzzyOutput = NULL;
};

CvFuzzyRule::~CvFuzzyRule() {
    if (fuzzyInput1 != NULL) {
        delete fuzzyInput1;
    }

    if (fuzzyInput2 != NULL) {
        delete fuzzyInput2;
    }

    if (fuzzyOutput != NULL) {
        delete fuzzyOutput;
    }
};

void CvFuzzyRule::setRule(CvFuzzyCurve* c1, CvFuzzyCurve* c2, CvFuzzyCurve* o1) {
    fuzzyInput1 = c1;
    fuzzyInput2 = c2;
    fuzzyOutput = o1;
};

double CvFuzzyRule::calcValue(double param1, double param2) {
    double v1, v2;
    v1 = fuzzyInput1->calcValue(param1);
    if (fuzzyInput2 != NULL) {
        v2 = fuzzyInput2->calcValue(param2);
        if (v1 < v2) {
            return v1;
        } else {
            return v2;
        }
    } else {
        return v1;
    }
};

CvFuzzyCurve* CvFuzzyRule::getOutputCurve() {
    return fuzzyOutput;
};

CvFuzzyController::CvFuzzyController() {
    // nothing to do
};

CvFuzzyController::~CvFuzzyController() {
    int size = (int)rules.size();
    for (int i = 0; i < size; i++) {
        delete rules[i];
    }
};

void CvFuzzyController::addRule(CvFuzzyCurve* c1, CvFuzzyCurve* c2, CvFuzzyCurve* o1) {
    CvFuzzyRule* f = new CvFuzzyRule();
    rules.push_back(f);
    f->setRule(c1, c2, o1);
};

double CvFuzzyController::calcOutput(double param1, double param2) {
    double v;
    CvFuzzyFunction list;
    int size = (int)rules.size();

    for (int i = 0; i < size; i++) {
        v = rules[i]->calcValue(param1, param2);
        if (v != 0) {
            list.addCurve(rules[i]->getOutputCurve(), v);
        }
    }
    v = list.calcValue();
    return v;
};

CvFuzzyMeanShiftTracker::FuzzyResizer::FuzzyResizer() {
    CvFuzzyCurve* i1L, *i1M, *i1H;
    CvFuzzyCurve* oS, *oZE, *oE;
    CvFuzzyCurve* c;

    double MedStart = 0.1, MedWidth = 0.15;

    c = iInput.newCurve();
    c->addPoint(0, 1);
    c->addPoint(0.1, 0);
    c->setCentre(0);
    i1L = c;

    c = iInput.newCurve();
    c->addPoint(0.05, 0);
    c->addPoint(MedStart, 1);
    c->addPoint(MedStart + MedWidth, 1);
    c->addPoint(MedStart + MedWidth + 0.05, 0);
    c->setCentre(MedStart + (MedWidth / 2));
    i1M = c;

    c = iInput.newCurve();
    c->addPoint(MedStart + MedWidth, 0);
    c->addPoint(1, 1);
    c->addPoint(1000, 1);
    c->setCentre(1);
    i1H = c;

    c = iOutput.newCurve();
    c->addPoint(-10000, 1);
    c->addPoint(-5, 1);
    c->addPoint(-0.5, 0);
    c->setCentre(-5);
    oS = c;

    c = iOutput.newCurve();
    c->addPoint(-1, 0);
    c->addPoint(-0.05, 1);
    c->addPoint(0.05, 1);
    c->addPoint(1, 0);
    c->setCentre(0);
    oZE = c;

    c = iOutput.newCurve();
    c->addPoint(-0.5, 0);
    c->addPoint(5, 1);
    c->addPoint(1000, 1);
    c->setCentre(5);
    oE = c;

    fuzzyController.addRule(i1L, NULL, oS);
    fuzzyController.addRule(i1M, NULL, oZE);
    fuzzyController.addRule(i1H, NULL, oE);
};

int CvFuzzyMeanShiftTracker::FuzzyResizer::calcOutput(double edgeDensity, double density) {
    return (int)fuzzyController.calcOutput(edgeDensity, density);
};

CvFuzzyMeanShiftTracker::SearchWindow::SearchWindow() {
    x = 0;
    y = 0;
    width = 0;
    height = 0;
    maxWidth = 0;
    maxHeight = 0;
    xGc = 0;
    yGc = 0;
    m00 = 0;
    m01 = 0;
    m10 = 0;
    m11 = 0;
    m02 = 0;
    m20 = 0;
    ellipseHeight = 0;
    ellipseWidth = 0;
    ellipseAngle = 0;
    density = 0;
    depthLow = 0;
    depthHigh = 0;
    fuzzyResizer = NULL;
};

CvFuzzyMeanShiftTracker::SearchWindow::~SearchWindow() {
    if (fuzzyResizer != NULL) {
        delete fuzzyResizer;
    }
}

void CvFuzzyMeanShiftTracker::SearchWindow::setSize(int _x, int _y, int _width, int _height) {
    x = _x;
    y = _y;
    width = _width;
    height = _height;

    if (x < 0) {
        x = 0;
    }

    if (y < 0) {
        y = 0;
    }

    if (x + width > maxWidth) {
        width = maxWidth - x;
    }

    if (y + height > maxHeight) {
        height = maxHeight - y;
    }
};

void CvFuzzyMeanShiftTracker::SearchWindow::initDepthValues(IplImage* maskImage, IplImage* depthMap) {
    unsigned int d = 0, mind = 0xFFFF, maxd = 0, m0 = 0, m1 = 0, mc, dd;
    unsigned char* data = NULL;
    unsigned short* depthData = NULL;

    for (int j = 0; j < height; j++) {
        data = (unsigned char*)(maskImage->imageData + (maskImage->widthStep * (j + y)) + x);
        if (depthMap) {
            depthData = (unsigned short*)(depthMap->imageData + (depthMap->widthStep * (j + y)) + x);
        }

        for (int i = 0; i < width; i++) {
            if (*data) {
                m0 += 1;

                if (depthData) {
                    if (*depthData) {
                        m1 += d;
                        if (d < mind) {
                            mind = d;
                        }
                        if (d > maxd) {
                            maxd = d;
                        }
                    }
                    depthData++;
                }
            }
            data++;
        }
    }

    if (m0 > 0) {
        mc = m1 / m0;
        if ((mc - mind) > (maxd - mc)) {
            dd = maxd - mc;
        } else {
            dd = mc - mind;
        }
        dd = dd - dd / 10;
        depthHigh = mc + dd;
        depthLow = mc - dd;
    } else {
        depthHigh = 32000;
        depthLow = 0;
    }
};

bool CvFuzzyMeanShiftTracker::SearchWindow::shift() {
    if ((xGc != (width / 2)) || (yGc != (height / 2))) {
        setSize(x + (xGc - (width / 2)), y + (yGc - (height / 2)), width, height);
        return true;
    } else {
        return false;
    }
};

void CvFuzzyMeanShiftTracker::SearchWindow::extractInfo(IplImage* maskImage, IplImage* depthMap, bool initDepth) {
    m00 = 0;
    m10 = 0;
    m01 = 0;
    m11 = 0;
    density = 0;
    m02 = 0;
    m20 = 0;
    ellipseHeight = 0;
    ellipseWidth = 0;

    maxWidth = maskImage->width;
    maxHeight = maskImage->height;

    if (initDepth) {
        initDepthValues(maskImage, depthMap);
    }

    unsigned char* maskData = NULL;
    unsigned short* depthData = NULL, depth;
    bool isOk;
    unsigned long count;

    verticalEdgeLeft = 0;
    verticalEdgeRight = 0;
    horizontalEdgeTop = 0;
    horizontalEdgeBottom = 0;

    for (int j = 0; j < height; j++) {
        maskData = (unsigned char*)(maskImage->imageData + (maskImage->widthStep * (j + y)) + x);
        if (depthMap) {
            depthData = (unsigned short*)(depthMap->imageData + (depthMap->widthStep * (j + y)) + x);
        }

        count = 0;
        for (int i = 0; i < width; i++) {
            if (*maskData) {
                isOk = true;
                if (depthData) {
                    depth = (*depthData);
                    if ((depth > depthHigh) || (depth < depthLow)) {
                        isOk = false;
                    }

                    depthData++;
                }

                if (isOk) {
                    m00++;
                    m01 += j;
                    m10 += i;
                    m02 += (j * j);
                    m20 += (i * i);
                    m11 += (j * i);

                    if (i == 0) {
                        verticalEdgeLeft++;
                    } else if (i == width - 1) {
                        verticalEdgeRight++;
                    } else if (j == 0) {
                        horizontalEdgeTop++;
                    } else if (j == height - 1) {
                        horizontalEdgeBottom++;
                    }

                    count++;
                }
            }
            maskData++;
        }
    }

    if (m00 > 0) {
        xGc = (m10 / m00);
        yGc = (m01 / m00);

        double a, b, c, e1, e2, e3;
        a = ((double)m20 / (double)m00) - (xGc * xGc);
        b = 2 * (((double)m11 / (double)m00) - (xGc * yGc));
        c = ((double)m02 / (double)m00) - (yGc * yGc);
        e1 = a + c;
        e3 = a - c;
        e2 = sqrt((b * b) + (e3 * e3));
        ellipseHeight = int(sqrt(0.5 * (e1 + e2)));
        ellipseWidth = int(sqrt(0.5 * (e1 - e2)));
        if (e3 == 0) {
            ellipseAngle = 0;
        } else {
            ellipseAngle = 0.5 * atan(b / e3);
        }

        density = (double)m00 / (double)(width * height);
    } else {
        xGc = width / 2;
        yGc = height / 2;
        ellipseHeight = 0;
        ellipseWidth = 0;
        ellipseAngle = 0;
        density = 0;
    }
};

void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsEdgeDensityLinear(int& resizeDx, int& resizeDy, int& resizeDw, int& resizeDh) {
    int x1 = horizontalEdgeTop;
    int x2 = horizontalEdgeBottom;
    int y1 = verticalEdgeLeft;
    int y2 = verticalEdgeRight;
    int gx = (width * 2) / 5;
    int gy = (height * 2) / 5;
    int lx = width / 10;
    int ly = height / 10;

    resizeDy = 0;
    resizeDh = 0;
    resizeDx = 0;
    resizeDw = 0;

    if (x1 > gx) {
        resizeDy = -1;
    } else if (x1 < lx) {
        resizeDy = +1;
    }

    if (x2 > gx) {
        resizeDh = resizeDy + 1;
    } else if (x2 < lx) {
        resizeDh = - (resizeDy + 1);
    } else {
        resizeDh = - resizeDy;
    }

    if (y1 > gy) {
        resizeDx = -1;
    } else if (y1 < ly) {
        resizeDx = +1;
    }

    if (y2 > gy) {
        resizeDw = resizeDx + 1;
    } else if (y2 < ly) {
        resizeDw = - (resizeDx + 1);
    } else {
        resizeDw = - resizeDx;
    }
};

void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsInnerDensity(int& resizeDx, int& resizeDy, int& resizeDw, int& resizeDh) {
    int newWidth, newHeight, dx, dy;
    double px, py;
    newWidth = int(sqrt(double(m00) * 1.3));
    newHeight = int(newWidth * 1.2);
    dx = (newWidth - width);
    dy = (newHeight - height);
    px = (double)xGc / (double)width;
    py = (double)yGc / (double)height;
    resizeDx = (int)(px * dx);
    resizeDy = (int)(py * dy);
    resizeDw = (int)((1 - px) * dx);
    resizeDh = (int)((1 - py) * dy);
};

void CvFuzzyMeanShiftTracker::SearchWindow::getResizeAttribsEdgeDensityFuzzy(int& resizeDx, int& resizeDy, int& resizeDw, int& resizeDh) {
    double dx1 = 0, dx2, dy1, dy2;

    resizeDy = 0;
    resizeDh = 0;
    resizeDx = 0;
    resizeDw = 0;

    if (fuzzyResizer == NULL) {
        fuzzyResizer = new FuzzyResizer();
    }

    dx2 = fuzzyResizer->calcOutput(double(verticalEdgeRight) / double(height), density);
    if (dx1 == dx2) {
        resizeDx = int(-dx1);
        resizeDw = int(dx1 + dx2);
    }

    dy1 = fuzzyResizer->calcOutput(double(horizontalEdgeTop) / double(width), density);
    dy2 = fuzzyResizer->calcOutput(double(horizontalEdgeBottom) / double(width), density);

    dx1 = fuzzyResizer->calcOutput(double(verticalEdgeLeft) / double(height), density);
    dx2 = fuzzyResizer->calcOutput(double(verticalEdgeRight) / double(height), density);
    //if (dx1 == dx2)
    {
        resizeDx = int(-dx1);
        resizeDw = int(dx1 + dx2);
    }

    dy1 = fuzzyResizer->calcOutput(double(horizontalEdgeTop) / double(width), density);
    dy2 = fuzzyResizer->calcOutput(double(horizontalEdgeBottom) / double(width), density);
    //if (dy1 == dy2)
    {
        resizeDy = int(-dy1);
        resizeDh = int(dy1 + dy2);
    }
};

bool CvFuzzyMeanShiftTracker::SearchWindow::meanShift(IplImage* maskImage, IplImage* depthMap, int maxIteration, bool initDepth) {
    numShifts = 0;
    do {
        extractInfo(maskImage, depthMap, initDepth);
        if (! shift()) {
            return true;
        }
    } while (++numShifts < maxIteration);

    return false;
};

void CvFuzzyMeanShiftTracker::findOptimumSearchWindow(SearchWindow& searchWindow, IplImage* maskImage, IplImage* depthMap, int maxIteration, int resizeMethod, bool initDepth) {
    int resizeDx, resizeDy, resizeDw, resizeDh;
    resizeDx = 0;
    resizeDy = 0;
    resizeDw = 0;
    resizeDh = 0;
    searchWindow.numIters = 0;
    for (int i = 0; i < maxIteration; i++) {
        searchWindow.numIters++;
        searchWindow.meanShift(maskImage, depthMap, MaxMeanShiftIteration, initDepth);
        switch (resizeMethod) {
        case rmEdgeDensityLinear :
            searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
            break;
        case rmEdgeDensityFuzzy :
            //searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
            searchWindow.getResizeAttribsEdgeDensityFuzzy(resizeDx, resizeDy, resizeDw, resizeDh);
            break;
        case rmInnerDensity :
            searchWindow.getResizeAttribsInnerDensity(resizeDx, resizeDy, resizeDw, resizeDh);
            break;
        default:
            searchWindow.getResizeAttribsEdgeDensityLinear(resizeDx, resizeDy, resizeDw, resizeDh);
        }

        searchWindow.ldx = resizeDx;
        searchWindow.ldy = resizeDy;
        searchWindow.ldw = resizeDw;
        searchWindow.ldh = resizeDh;

        if ((resizeDx == 0) && (resizeDy == 0) && (resizeDw == 0) && (resizeDh == 0)) {
            break;
        }

        searchWindow.setSize(searchWindow.x + resizeDx, searchWindow.y + resizeDy, searchWindow.width + resizeDw, searchWindow.height + resizeDh);
    }
};

CvFuzzyMeanShiftTracker::CvFuzzyMeanShiftTracker() {
    searchMode = tsSetWindow;
};

CvFuzzyMeanShiftTracker::~CvFuzzyMeanShiftTracker() {
    // nothing to do
};

void CvFuzzyMeanShiftTracker::track(IplImage* maskImage, IplImage* depthMap, int resizeMethod, bool resetSearch, int minKernelMass) {
    bool initDepth = false;

    if (resetSearch) {
        searchMode = tsSetWindow;
    }

    switch (searchMode) {
    case tsDisabled:
        return;
    case tsSearching:
        return;
    case tsSetWindow:
        kernel.maxWidth = maskImage->width;
        kernel.maxHeight = maskImage->height;
        kernel.setSize(0, 0, maskImage->width, maskImage->height);
        initDepth = true;
    case tsTracking:
        searchMode = tsSearching;
        findOptimumSearchWindow(kernel, maskImage, depthMap, MaxSetSizeIteration, resizeMethod, initDepth);
        if ((kernel.density == 0) || (kernel.m00 < minKernelMass)) {
            searchMode = tsSetWindow;
        } else {
            searchMode = tsTracking;
        }
    }
};

